
#include <Arduino.h>
#include <Wire.h>
#include <Servo.h>
#include <SoftwareSerial.h>

#include "mBot.h"
#include "MePort.h"
#include "MeRGBLed.h"
#include "MeUltrasonic.h"
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <NewTone.h>

double angle_rad = PI/180.0;
double angle_deg = 180.0/PI;

MeBoard myBoard(mBot);

MeRGBLed rgbled_7(7,7==7?2:4);

MeUltrasonic ultrasonic_3(3);

LiquidCrystal_I2C mylcd(0x27,16,2);

int loop_num = 0;

void setup(){
  mylcd.init();
  mylcd.backlight();
  pinMode(8, OUTPUT);
}

void measure_disp_distance() {
  char buf[32];
  int dis = ultrasonic_3.distanceCm();
  sprintf(buf, "distance: %d cm", dis);
  mylcd.setCursor(0, 1);
  mylcd.print(buf);
}

void loop() {
  char buf[32];

  sprintf(buf, "loop: %d", loop_num++);
  mylcd.setCursor(0, 0);
  mylcd.print(buf);

  measure_disp_distance();

  // section1
  rgbled_7.setColor(0,255,0,0);
  rgbled_7.show();
  delayMicroseconds(100);
  NewTone(8,262,500);
  NewTone(8,262,500);

  measure_disp_distance();

  // section2
  rgbled_7.setColor(0,0,255,0);
  rgbled_7.show();
  delayMicroseconds(100);
  NewTone(8,392,500);
  NewTone(8,392,500);

  measure_disp_distance(); 

  // section3
  rgbled_7.setColor(0,0,0,255);
  rgbled_7.show();
  delayMicroseconds(100);
  NewTone(8,440,500);
  NewTone(8,440,500);
  NewTone(8,392,500);
}
